
import cv2   #导入库
import numpy as np
import time
import threading
import pigpio
import os

from math import *




text_lower = np.array([0,0,30])
text_upper = np.array([200,43,70]) #设置test区间

color_lower = blue_lower    #选择要识别的颜色
color_upper = blue_upper

#引脚定义
PIN_yuntai = 26
PIN_camera = 12
#全局变量定义
pwm_bias1 = 0   
pwm_value1 = 1500
pwm_bias2 = 0   
pwm_value2 = 1500

systick_ms_bak = 0


pi = pigpio.pi()

width = 320
hight = 240

cap = cv2.VideoCapture(0)  #打开摄像头
cap.set(3,width)
cap.set(4,hight)



pi.set_servo_pulsewidth(PIN_yuntai, pwm_value1)
pi.set_servo_pulsewidth(PIN_camera, pwm_value2)
    
#摄像头跟随
def camera_follow(c_x,c_y):
    global systick_ms_bak,pwm_value1,pwm_value2
    if(int((time.time() * 1000))- systick_ms_bak >= 100):
        systick_ms_bak = int((time.time() * 1000))
        x_bias = c_x - width/2
        y_bias = c_y - hight/2
        if abs(x_bias) < 30 and abs(y_bias) < 30:
            pass
        else: 
            pwm_value1 = pwm_value1 - x_bias
            pwm_value2 = pwm_value2 + y_bias
            pi.set_servo_pulsewidth(PIN_yuntai, pwm_value1)
            pi.set_servo_pulsewidth(PIN_camera, pwm_value2)


while 1: 
    
    ret,frame = cap.read()

    #高斯滤波
    frame = cv2.GaussianBlur(frame,(5,5),0)
    #将图片的色域转换为HSV
    hsv = cv2.cvtColor(frame,cv2.COLOR_BGR2HSV)

    mask = cv2.inRange(hsv,color_lower,color_upper)  #设置阈值

    mask = cv2.erode(mask,None,iterations=2)
    
    #高斯模糊
    mask = cv2.GaussianBlur(mask,(3,3),0)
    
    #图像合并
    res = cv2.bitwise_and(frame,frame,mask=mask) 

    #边缘检测
    cnts = cv2.findContours(mask.copy(),cv2.RETR_EXTERNAL,cv2.CHAIN_APPROX_SIMPLE)[-2] 
    #print("time3:",time.time())
    if len(cnts) >0 : #通过边缘检测来确定所识别物体的位置信息得到相对坐标

        cnt = max(cnts,key=cv2.contourArea)
        rect = cv2.minAreaRect(cnt)
        # 获取最小外接矩形
        box = cv2.boxPoints(rect)
                    
        #获取坐标 长宽 角度
        c_x, c_y = rect[0]
        c_h, c_w = rect[1]
        c_angle = rect[2]
        if c_angle<-45:
            c_angle = -(90+c_angle)

            #绘制轮廓
            cv2.drawContours(frame, [np.int0(box)], -1, (0, 255, 255), 2)
            

            camera_follow(c_x,c_y)

                                                                                                       
    cv2.imshow('frame',frame) #将具体的测试效果显示出来

    if cv2.waitKey(5) & 0xFF == 27: #ESC退出 
        break

cap.release()
cv2.destroyAllWindows()

